The present invention is generally related to vehicular forward path geometry estimation, and more specifically to a method and apparatus for accurate estimation of vehicle forward path geometry utilizing an adaptive Kalman filter bank and a two-clothoid road model.
Vehicular land and road detection systems have been the subject of significant research. Applications such as adaptive cruise control, collision avoidance systems, and vehicle guidance systems require knowledge of the geometry of the road to be most effective. It is difficult for a collision avoidance system to accurately characterize a stationary or slower moving object as non-hazardous unless there is some certainty that the stationary object is not in the vehicle""s path. Previous approaches to estimation of forward road geometry model the road in the forward view as either a constant curvature or a linearly varying curvature. These road geometry estimation approaches then fit parameters of these models via a least-squares fitting technique or other mathematical technique. The linearly-varying curvature model is usually referred to as a single-clothoid model and is completely described by two coefficients: c0, the local curvature at host vehicle position, and c1, the rate of change of curvature with distance. Both the constant curvature and single-clothoid road models do not adequately represent road geometry, especially when there are abrupt changes in curvature in the look-ahead range. Such sharp changes in curvature are common on many roads and freeways, particularly where a straightaway section abruptly transitions to a curved section and vice-versa. Thus, methods based on these simple models are generally inadequate in describing forward road geometry.
To overcome the problem of inaccurate road geometry estimation with these simple models, an averaged curvature road model has been proposed. This approach works well on roads with low curvature and very smooth curvature changes, but still fails on roads with significant curvature changes or discontinuities in c1. To reduce these errors further, more complex road models were proposed that split the road into multiple clothoid segments. The transition between segments is estimated using ad-hoc methods such as the generalized likelihood ratio test. In this approach, the geometry of the segments is not dynamic, and is updated using measurements that fall in the corresponding segment. A dynamic model is required for vehicle dynamics only, as the road model remains spatially fixed, while the vehicle is moving through the road segments. One of the fundamental limitations of this approach is that road geometry estimation accuracy is strongly dependent on reliable detection of transition points between segments making it very sensitive to noise in the measurements. Thus they are often inaccurate in moderate and high noise measurement conditions. In addition, the segmentation process is computationally expensive because the likelihood ratio test has to be performed for each new measurement.
A naxc3xafve approach of using a road model based on a higher-order polynomial to solve these problems with previous methods will lead to over-fitting road geometry and a statistically insignificant estimate of the coefficient of this higher order term. This will also make this approach more sensitive to noise. Thus, it would be desirable to develop a model that overcomes the problems associated with previous approaches. Ideally such a model would be simple to implement and would not be as computationally intensive or ad-hoc as the previous multi-clothoid models.
Previous approaches use a conventional Kalman filter with fixed process model parameters that are chosen to provide a compromise between noise and filter lag. In other words, process noise parameters in the conventional filters is chosen so that the filter lag (time taken by filter to catch up with new geometry) is reduced during transitions in road geometry at the expense of introducing some noise during slowly changing road geometry conditions. Thus, the filter performance is moderate during all road geometry conditions. However, the estimation errors may still be unacceptably large for systems that rely on the forward geometry information.
The single-clothoid road model could result in large estimation errors on roads with transitions in curvature rate parameter c1 in the look-ahead range. To overcome the problem of suddenly changing c1 parameters, artisans have used an averaged curvature model. This approach is suitable on some ideal roads with low curvature and very smooth curvature changes, but performs poorly on roads with significant jumps in c1. To reduce these errors, the road model was split into a number of segments and each segment was modeled as a clothoid. However, this solution assumes that clothoid coefficients c0 and c1 are constant instead of being time-dependent. As a consequence, measurements only improve the accuracy of estimation of segment parameters, but do not drive a dynamic system of the road course. This approach requires accurate estimation of the transition between road segments. Artisans have estimated transition points by applying spatially recursive estimation and the generalized likelihood ratio method. One of the fundamental limitations of this approach is that road geometry estimation accuracy is strongly dependent on reliable detection of transition points which is a very difficult estimation problem. In addition, the segmentation process is computationally expensive because the likelihood ratio test has to be performed for each new measurement. It would thus be desirable to provide a geometry estimation scheme that is not strongly dependent on reliable detection of transition points and is not computationally expensive. It would also be desirable to avoid the need to use compromised, static process model parameters.
The present invention provides an apparatus for dynamically predicting road geometry. The apparatus comprises a bank of filters configured to receive current environmental data from at least one sensor. After receiving the data, the bank of filters performs an estimation using the data, resulting in filter output from each filter. A weighting element is configured to receive the filter output from the bank of filters, and to ascribe a weighted value to each filter output based on a relevance of the filter. The weighting element thereby provides weighted outputs.
The apparatus further comprises a fusing element configured to fuse the weighted outputs into a single output; whereby the output describes a vehicle forward path estimate.
In another aspect, the bank of filters of the apparatus is further configured to receive a previous vehicle path estimate and to recursively perform the estimation based on the current environmental data and the previous vehicle path estimate. In a still further aspect, each filter in the bank of filters is a Kalman filter. In yet another aspect, the bank of filters includes three Kalman filters.
In a further aspect, the configuration of the bank of filters is based on a polynomial road model, which in a still further aspect is a two-clothoid road model. In this case, each filter may be tuned to a different class of geometry within the two-clothoid road model. In a more specific aspect, the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.
In yet another aspect, the weighting element ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.
In a further aspect, the sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.
In another aspect, the output of the apparatus is configured for use by other automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.
Each of the operations of the apparatus discussed above typically corresponds to a software module for performing the function on a computer. In other embodiments, the means or modules may be incorporated onto a computer readable medium to provide a computer program product. Also, the means discussed above also correspond to steps in a method for estimating a vehicle forward path.